//
// Created by ze on 2024/1/3.
//
#include "UPRE_Hand_Shank.h"
#include "usart.h"
#include "UPRE_PID.h"
#include "UPRE_Decision.h"
//手柄数据
Hand_Shank_ Hand_Shank_Data = {0};
uint8_t begin_Chassic=0;
uint8_t body_spin=0;
uint8_t Receive_x=0;
uint8_t Receive_y=0;
uint8_t Color_Num=0;
float Angle=0.f;
//蓝牙接收数组
extern uint8_t recbuff[100];
union Switch_UINT8_T2Float_{
    uint8_t P[4];
    float value;
}Switch_UINT8_T2Float;
//蓝牙接受函数
void       Start_Hand_Shank_Receive(void)
{
//    printf("l_x:%d\t l_y:%d\t r_x:%d\t r_y:%d \r\n",Hand_Shank_Data.left_x,Hand_Shank_Data.left_y,Hand_Shank_Data.right_x,Hand_Shank_Data.right_y);
//    printf("dir:%f\t  vel:%f\t Yaw:%f\r\n",Hand_Shank_Data.Vel_Hand_Shank.dir,Hand_Shank_Data.Vel_Hand_Shank.size,Hand_Shank_Data.Pose_Hand_Shank.yaw);
    /**********************************************计算校验和******************************************************/
    if(recbuff[0]==0x3A && recbuff[31]==0xA3) {
        Hand_Shank_Data.A =      recbuff[1];
        Hand_Shank_Data.B =      recbuff[2];
        Hand_Shank_Data.C =      recbuff[3];
        Hand_Shank_Data.D =      recbuff[4];
        Hand_Shank_Data.E =      recbuff[5];
        Hand_Shank_Data.F =      recbuff[6];
        Hand_Shank_Data.G =      recbuff[7];
        Hand_Shank_Data.H =      recbuff[8];
        Hand_Shank_Data._2 =     recbuff[9];
        Hand_Shank_Data._3 =     recbuff[10];
        Hand_Shank_Data._4 =     recbuff[11];
        Hand_Shank_Data.R =      recbuff[12];
        Hand_Shank_Data.Add =    recbuff[13];
        Hand_Shank_Data.Minus =  recbuff[14];
        Hand_Shank_Data.LEFT =   recbuff[15];
        Hand_Shank_Data._1=      recbuff[16];
        Hand_Shank_Data.RIGHT =  recbuff[17];
        Hand_Shank_Data.L =      recbuff[18];
        Switch_UINT8_T2Float.P[0] = recbuff[19];
        Switch_UINT8_T2Float.P[1] = recbuff[20];
        Switch_UINT8_T2Float.P[2] = recbuff[21];
        Switch_UINT8_T2Float.P[3] = recbuff[22];
        Hand_Shank_Data.Vel_Hand_Shank.dir = Switch_UINT8_T2Float.value;//速度方向
        Switch_UINT8_T2Float.P[0] = recbuff[23];
        Switch_UINT8_T2Float.P[1] = recbuff[24];
        Switch_UINT8_T2Float.P[2] = recbuff[25];
        Switch_UINT8_T2Float.P[3] = recbuff[26];
        Hand_Shank_Data.Vel_Hand_Shank.size = Switch_UINT8_T2Float.value;//速度大小
        Switch_UINT8_T2Float.P[0] = recbuff[27];
        Switch_UINT8_T2Float.P[1] = recbuff[28];
        Switch_UINT8_T2Float.P[2] = recbuff[29];
        Switch_UINT8_T2Float.P[3] = recbuff[30];
        Hand_Shank_Data.Pose_Hand_Shank.yaw = Switch_UINT8_T2Float.value+Angle;//偏航角
        begin_Chassic=Hand_Shank_Data._3;
        body_spin=Hand_Shank_Data.RIGHT;
        Pack_receive.is_shoot_ball=Hand_Shank_Data.H;
        Pack_receive.is_eat_ball  =Hand_Shank_Data.G;
        Pack_receive.is_accelerate=Hand_Shank_Data.Add;
        Pack_receive.is_decelerate=Hand_Shank_Data.Minus;
    }

    if(begin_Chassic==1)
    {
        Pack_receive.is_start_chassic=true;
    }
    if(Hand_Shank_Data._1 == 1)
    {
        Hand_Shank_Data.Flag_Start_HandShank = 1;
    }
//    else if(Hand_Shank_Data._2 == 1)
//    {
//        Hand_Shank_Data.Flag_Start_HandShank = 0;
//    }
    if(Hand_Shank_Data.A)
    {
        Receive_x=1;
    }
    if(Hand_Shank_Data.C)
    {
        Receive_x=2;
    }
    if(Hand_Shank_Data.E)
    {
        Receive_x=3;
    }
    if(Hand_Shank_Data.B)
    {
        Receive_x=4;
    }
    if(Hand_Shank_Data.D)
    {
        Receive_x=5;
    }
    if(Hand_Shank_Data.F)
    {
        Receive_x=6;
    }
    if(Hand_Shank_Data._2)
    {
        Receive_y=1;
    }
    if(Hand_Shank_Data._4)
    {
        Receive_y=2;
    }
    if(Receive_x!=0&&Receive_y!=0)
    {
        if(Color_Num<6) {
            //数组赋值
            Receive_x = 0;
            Receive_y = 0;
            Color_Num++;
        }

    }

    if(Hand_Shank_Data.Flag_Start_HandShank) {
        setPidCalMode(PID_YAW_ONLY);
        setPidTargetPose(&Hand_Shank_Data.Pose_Hand_Shank, PID_YAW_ONLY);
    }
//    }


    /************************************************************************************************************/

}
//void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
//{
//    if(huart->Instance == USART3)
//    {
//
//        if (temp == 0x3A)
//        {
//            flag_cal_sum = true;
//            Num=0;
//        }
//        if(flag_cal_sum == true)
//        {
//            if(Num>0 && Num<27)
//                sum += temp;
//            Hand_Shank_Recv[Num] = temp;
//            Num++;
//            if(Num>=27) {
//                flag_cal_sum = false;
//                sum = 0;
//            }
//        }
//        HAL_UART_Receive_IT(&huart3,&temp,1);
//    }
//}